%%
%三组数据，滤波，原始数据dis
clf;
dis_butter=filter(filter_fun,dis);
hold on;
i_start=1400;
i_end=1600;
plot(t_relative(i_start:i_end),dis_butter(i_start:i_end,1),'k','LineWidth',2);
plot(t_relative(i_start:i_end),dis_mean(i_start:i_end,1),'r','LineWidth',2)
plot(t_relative(i_start:i_end),dis_sg(i_start:i_end,1),'b','LineWidth',2)
plot(t_relative(i_start:i_end),dis(i_start:i_end,1),'g','LineWidth',2);
legend('butter滤波器','mean(0.9+0.1)','sg滤波','原数据','fontSize',12)
xlabel('t/s');
ylabel('dis1/m');
figure_FontSize=16;
set(get(gca,'XLabel'),'FontSize',figure_FontSize,'Vertical','top');
set(get(gca,'YLabel'),'FontSize',figure_FontSize,'Vertical','middle');
set(findobj('FontSize',12),'FontSize',figure_FontSize);
% plot(t_relative,dis(:,1),'k');
% plot(t_relative,dis_old(:,1),'r');
% plot(t_relative,dis_butter(:,1),'r');
% plot(t_relative,dis_mean(:,1),'g');

%%
%解算后的位姿
clf;
col=1;
subplot(3,1,1);
hold on
% plot(t_relative_UWB, pose_U_y);%之前解出来的
% plot(t_relative,p_new_ori(:,2),'r');
% plot(t_relative,p_new_butter(:,col),'r');
plot(t_relative,p_new_sg(:,col),'b');
% plot(t_relative,p_sg_butter(:,col),'k');
% plot(t_relative,p_new_sg_3_11(:,col),'r');
% plot(t_relative,p_new_sg_3_31(:,col));
% plot(t_relative,p_new_sg_7_31(:,col));
plot(t_relative_icon, pose_i_x);
legend('pose_{sg}','vicon');
xlabel('t/s');
ylabel('x/m');
% legend('pose_{sg}','pose_{sg3-11}','pose_{sg3-31}','pose_{sg7-31}','pose_{vicon}')

subplot(3,1,2);
hold on
plot(t_relative,p_new_sg(:,2),'b');
plot(t_relative_icon, pose_i_y);
legend('pose_{sg}','vicon');
xlabel('t/s');
ylabel('y/m');
subplot(3,1,3);
hold on
plot(t_relative,p_new_sg(:,3),'b');
plot(t_relative_icon, pose_i_z);
legend('pose_{sg}','vicon');
xlabel('t/s');
ylabel('z/m');
%%
clf;
hold on;
%p_uwb有3541个点;
%p_vicon有4551个点  20-50 105-135
for i=1:2
    if i==1
       t_start=25;
       t_end=50; 
    end
    if i==2
      t_start=105;
      t_end=130;  
    end
    subplot(1,2,i);
    hold on;
    p_start=fix(t_start/141.6*3541);
    p_end=fix(t_end/141.6*3541);
    p_start_v=fix(t_start/151.667*4551);
    p_end_v=fix(t_end/151.667*4551);
    plot(p_new_sg(p_start:p_end,1),p_new_sg(p_start:p_end,2),'b');
    plot(pose_i_x(p_start_v:p_end_v), pose_i_y(p_start_v:p_end_v));
    legend('pose_{sg}','vicon');
    xlabel('x/m');
    ylabel('y/m');
    if i==1
        title('第25-50s的xy图');
    else
        title('第105-135s的xy图');
    end
end